#include "FrankaRobotControl.h"

#include <QDebug>
#include <QMutexLocker>
#include <QElapsedTimer>
#include <QTimer>
#include <QEventLoop>
#include <limits>

#include <errno.h>
#include <signal.h>
#include <stdio.h>
#include <string.h>
#include <sched.h>
#include <sys/resource.h>
#include <sys/time.h>
#include <sys/types.h>
#include <unistd.h>
#include <time.h>
#include <sys/mman.h>
#include <malloc.h>
#include <stdlib.h>
#include <pthread.h>
#include <stdarg.h>
#include <sys/ipc.h>
#include <sys/msg.h>
#include <pthread.h>



#if 0
//8MS-针对不同机器人的扫描周期
int PERIOD_NS_franka = 1000000;//1000000
#define     NSEC_PER_SEC_franka         (1000000000L)
#define  CLOCK_TO_USE  CLOCK_REALTIME
#define  pi 3.14159265357

bool FrankaRobotControl::ExtFollowerIFStartTrack=false;//因为franka的特殊写法，只能适应他这个，做成静态的，否则他死循环的时候无法读写非静态变量
bool FrankaRobotControl::ExtGetStart=false;//因为franka的特殊写法，只能适应他这个，做成静态的，否则他死循环的时候无法读写非静态变量
int FrankaRobotControl::tick=0;
int FrankaRobotControl::command=0;
double FrankaRobotControl::command_J0=0;
double FrankaRobotControl::command_J1=0;
double FrankaRobotControl::command_J2=0;
double FrankaRobotControl::command_J3=0;
double FrankaRobotControl::command_J4=0;
double FrankaRobotControl::command_J5=0;
double FrankaRobotControl::command_J6=0;

double FrankaRobotControl::testvalueX=0;
double FrankaRobotControl::testvalueY=0;
double FrankaRobotControl::testvalueZ=0;

Eigen::Matrix<double,6,7>  Jmatrix(std::array<double, 7> qsq,Eigen::Matrix4d Tthat);
Eigen::Matrix<double,6,7> JJJmatrix(Eigen::VectorXd qsq,Eigen::Matrix4d Tlast,MathWay* mathway);


struct timespec timespec_add_franka(struct timespec time1, struct timespec time2)
{
    struct timespec result;

    if ((time1.tv_nsec + time2.tv_nsec) >= NSEC_PER_SEC_franka) {
        result.tv_sec = time1.tv_sec + time2.tv_sec + 1;
        result.tv_nsec = time1.tv_nsec + time2.tv_nsec - NSEC_PER_SEC_franka;
    } else {
        result.tv_sec = time1.tv_sec + time2.tv_sec;
        result.tv_nsec = time1.tv_nsec + time2.tv_nsec;
    }

    return result;
}

FrankaRobotControl::FrankaRobotControl(QString Name)
{
    RobotName=Name;

    //不要直接在Start里面写运行内容，而是放到run函数里面，否则会卡死
    //this->start();
}

FrankaRobotControl::~FrankaRobotControl()
{

}



/*方向求解，向量叉乘*/
Eigen::Vector3d  ThreeCross(Eigen::Vector3d a,Eigen::Vector3d b){

    Eigen::Vector3d C;
    C(0)=a(1)*b(2)-a(2)*b(1);
    C(1)=a(2)*b(0)-a(0)*b(2);
    C(2)=a(0)*b(1)-a(1)*b(0);
    double L=sqrt(pow(C(0),2)+pow(C(1),2)+pow(C(2),2));
    C(0)=C(0)/L;
    C(1)=C(1)/L;
    C(2)=C(2)/L;
    return C;
}

/*RCM约束*/
Eigen::Vector3d RCM (Eigen::Vector3d A, Eigen::Vector3d O,double l){
    Eigen::Vector3d B;
    double d=0;
    for(int i=0;i<3;i++){
        d +=  (A(i)-O(i)) * (A(i)-O(i));
    }
    d=sqrt(d);
    for(int i=0;i<3;i++){
        B(i)=O(i)+(l-d)*((O(i)-A(i))/d);
    }
    return B;
}


//多线程的调用执行方法
void FrankaRobotControl::run_code()
{
    FrankaJMatrix* JMatrix = new FrankaJMatrix;
    FrankaCameraProc* Camera = new FrankaCameraProc;


    //应该在这个类的实例化的时候，开启实时线程(但是可以什么也不做)
    //   runSomeBigWork0();
    //    runJointWork3();
    //     runJointWork1();
    test_hahaha();
    std::cout << "------------------------------------------" << std::endl;

    //    test_dynamicRCM();
    //    test_dynamicRCM_copy();
}

bool FrankaRobotControl::robotLogin(QString addr, int port)
{
    //他这个应该每次要获取都得实例化一个，如果这个IP地机器人没有，则直接挂了
    //franka::Robot robot(RobotIP);//"192.168.65.110"

    //use pointer can avoid init instance in all function
    //robot= new franka::Robot("192.168.65.110");
    robot= new franka::Robot(addr.toStdString());

    RobotPort=port;

    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    IfConnected=true;
    ExtGetStart=true;

    std::cout<<"FrankaRobotControlV2::robotLogin2 IfConnected="<<IfConnected<<std::endl;
    return IfConnected;
}

bool FrankaRobotControl::robotLogout()
{
    //退出死循环
    IfExit=true;

    return false;
}

//是否有直接给机器人发指令的说法？
void FrankaRobotControl::robotSendCommand(QString cmd)
{
    //是否支持？
}

void FrankaRobotControl::robotForward(double jointangle[6], Eigen::Matrix3d& robot_ori, Eigen::Vector3d& robot_trans)
{
    //是否支持？
}

void FrankaRobotControl::robotInverse(Eigen::Matrix4d trans_matrix, double* joint_target)
{
    //是否支持？
}

void FrankaRobotControl::robotMoveJ(LREALArray10 pos)
{

    command_J0=pos.DATA[0];
    command_J1=pos.DATA[1];
    command_J2=pos.DATA[2];
    command_J3=pos.DATA[3];
    command_J4=pos.DATA[4];
    command_J5=pos.DATA[5];
    command_J6=pos.DATA[6];
    command=1;

    // 因为已经有线程死循环了，所以只能放到那个线程去决定做什么，采集位置或者控制运动
    //      try {
    //        //setDefaultBehavior(robot);
    //        std::array<double, 7> q_goal;
    //        for (size_t i = 0; i < 7; i++) {
    //            //q_goal[i] = std::stod(argv[i + 2]);
    //            q_goal[i] = pos.DATA[i];
    //        }
    //        double speed_factor = 0.5;//std::stod(argv[9])

    //        // Set additional parameters always before the control loop, NEVER in the control loop!
    //        // Set collision behavior.
    //        //        robot->setCollisionBehavior(
    //        //            {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}},
    //        //            {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}},
    //        //            {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}},
    //        //            {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}});

    //        MotionGenerator motion_generator(speed_factor, q_goal);
    //        //        std::cout << "WARNING: This example will move the robot! "
    //        //                  << "Please make sure to have the user stop button at hand!" << std::endl
    //        //                  << "Press Enter to continue..." << std::endl;
    //        //std::cin.ignore();
    //        robot->control(motion_generator);
    //        std::cout << "Motion finished" << std::endl;
    //      } catch (const franka::Exception& e) {
    //        std::cout << e.what() << std::endl;
    //    }


}

void FrankaRobotControl::robotMoveL(Eigen::Matrix4d desired_EndMatrix)
{
    //franka的非实时TCP运动指令(no)


}

//获取通用的单周期数据包
//use this
static double testdata=0;
Eigen::Matrix4d FrankaRobotControl::robotGetRealtimeDATA()
{
    return RobotPoseRealTime;
}

//获取通用的单周期数据包(pay attention to run speed!!!)
//if can do then use this(more infomation than tcp matricx)
SRobotRealTimeDataV2 FrankaRobotControl::robotGetRealtimeDATAV2()
{
    //not done!!!
    return RealtimeDATA;

    //    int count=0;
    //    double testdata=0;
    //    franka::RobotState robot_stateTEST;

    //    std::cout<<"SRobotRealTimeDataV2 FrankaRobotControlV2::robotGetRealtimeDATA() 1"<<std::endl;

    //    //,&testdata
    //    //,&robot_stateTEST
    //    robot->read([&count](const franka::RobotState& robot_state) {
    //                      // Printing to std::cout adds a delay. This is acceptable for a read loop such as this, but
    //                      // should not be done in a control loop.
    //                      std::cout <<"FrankaRobotControlV2::AAAAAAAAAAAAAAAAAAA="<< robot_state<< std::endl;

    //                      //testdata=robot_state.O_T_EE[0];
    //                      //robot_stateTEST=robot_state;

    //                      return count++ < 1;
    //                  });

    //std::cout<<"SRobotRealTimeDataV2 FrankaRobotControlV2::robotGetRealtimeDATA() 2"<<std::endl;

    //    RealtimeDATA.TCPMatrix(0,0)=robot_stateTEST.O_T_EE[0];
    //    RealtimeDATA.TCPMatrix(1,0)=robot_stateTEST.O_T_EE[1];
    //    RealtimeDATA.TCPMatrix(2,0)=robot_stateTEST.O_T_EE[2];
    //    RealtimeDATA.TCPMatrix(3,0)=robot_stateTEST.O_T_EE[3];

    //    RealtimeDATA.TCPMatrix(0,1)=robot_stateTEST.O_T_EE[4];
    //    RealtimeDATA.TCPMatrix(1,1)=robot_stateTEST.O_T_EE[5];
    //    RealtimeDATA.TCPMatrix(2,1)=robot_stateTEST.O_T_EE[6];
    //    RealtimeDATA.TCPMatrix(3,1)=robot_stateTEST.O_T_EE[7];

    //    RealtimeDATA.TCPMatrix(0,2)=robot_stateTEST.O_T_EE[8];
    //    RealtimeDATA.TCPMatrix(1,2)=robot_stateTEST.O_T_EE[9];
    //    RealtimeDATA.TCPMatrix(2,2)=robot_stateTEST.O_T_EE[10];
    //    RealtimeDATA.TCPMatrix(3,2)=robot_stateTEST.O_T_EE[11];

    //    RealtimeDATA.TCPMatrix(0,3)=robot_stateTEST.O_T_EE[12];
    //    RealtimeDATA.TCPMatrix(1,3)=robot_stateTEST.O_T_EE[13];
    //    RealtimeDATA.TCPMatrix(2,3)=robot_stateTEST.O_T_EE[14];
    //    RealtimeDATA.TCPMatrix(3,3)=robot_stateTEST.O_T_EE[15];


    //franka可以调用一次获取数据包，但是他这么写就必须每个周期产生类的实例，不合理(其实可以放在他的实时线程里面，如果没有任何控制指令，则每个周期获取数据并更新自身的变量值)

    //    franka::Robot robotAAA(RobotIP);
    //    try {

    //        franka::RobotState robot_state=robotAAA.readOnce();
    //        std::cout<<"FrankaRobotControlV2::robotGetRealtimeDATA"<<std::endl;
    //        std::cout<<"robot_state="<<robot_state<<std::endl;

    //    }
    //    catch (franka::Exception const& e) {
    //        std::cout << e.what() << std::endl;
    //        std::cout << "Running error recovery..." << std::endl;
    //        robotAAA.automaticErrorRecovery();

    //    }

}

//跟随参数设置
void FrankaRobotControl::robotFollowSet(RobotFollowPara followparameter)
{
    std::cout<<"FrankaRobotControlV2::robotFollowSet1"<<std::endl;

    //初始化主从跟随参数(跟随速度，跟随策略等)
    FollowMode=followparameter.FollowMode;
    FollowSource=followparameter.FollowSource;
    FollowSourceIndex=followparameter.FollowSourceIndex;
    Followstrategy=followparameter.Followstrategy;

    std::cout<<"FrankaRobotControlV2::robotFollowSet2"<<std::endl;

    std::cout<<"FollowMode="<<(int)FollowMode<<std::endl;
    std::cout<<"FollowSource="<<(int)FollowSource<<std::endl;
    std::cout<<"Followstrategy="<<(int)Followstrategy<<std::endl;
    std::cout<<"FollowSourceIndex="<<FollowSourceIndex<<std::endl;

}

//开启跟随
void FrankaRobotControl::robotFollowStart()
{
    for(int i=0;i<6;i++)
    {
        StartJoint.DATA[i]=RobotJointPos[i];
        StartTCP.DATA[i]=RobotTCPPos[i];
    }

    std::cout<<"RobotJointPos="<<RobotJointPos[0]<<","<<RobotJointPos[1]<<","<<RobotJointPos[2]<<","<<std::endl;
    std::cout<<"RobotTCPPos="<<RobotTCPPos[0]<<","<<RobotTCPPos[1]<<","<<RobotTCPPos[2]<<","<<std::endl;

    //可以以sin曲线方式测试跟随
    ExtFollowerIFStartTrack=true;

}

//停止跟随
void FrankaRobotControl::robotFollowStop()
{
    //停止跟随涉及平滑停止问题
    ExtFollowerIFStartTrack=false;
}

//设置跟随值（测试）
void FrankaRobotControl::robotFollowSetTESTX(double newvalue)
{
    testvalueX=newvalue;
}

void FrankaRobotControl::robotFollowSetTESTY(double newvalue)
{
    testvalueY=newvalue;
}

void FrankaRobotControl::robotFollowSetTESTZ(double newvalue)
{
    testvalueZ=newvalue;
}

//设置下一步要走的joint值
void FrankaRobotControl::robotFollowSetJoint(LREALArray10 targetjointpos)
{
    for(int i=0;i<6;i++)
    {
        FinalJoint.DATA[i]=targetjointpos.DATA[i];
    }
}

//设置下一步要走的tcp值
void FrankaRobotControl::robotFollowSetTCP(Eigen::Matrix4d desired_EndMatrix)
{
    //把输入的44矩阵转成UR能识别的XYZABC格式(姿态轴角)
    //FinalTCP
}



void FrankaRobotControl::runSomeBigWork3()
{

    //with RCM in world with camera

    if(true)
    {
        pid_t pid = getpid();
        if (setpriority(PRIO_PROCESS, pid, -19))
            fprintf(stderr, "Warning: Failed to set priority: %s\n",strerror(errno));

        /* Create cyclic RT-thread */
        struct sched_param param;
        param.sched_priority = sched_get_priority_max(SCHED_FIFO) - 1;
        if (sched_setscheduler(0, SCHED_FIFO, &param) == -1) {
            printf("ERROR IN SETTING THE SCHEDULER");
            perror("errno");
            //return -1;
        }

        if(false)//fun_globalvariable::instance()->IfWorkModeReal==true
        {
            cpu_set_t cpuset;
            CPU_ZERO(&cpuset);
            CPU_SET(3, &cpuset);
            if (sched_setaffinity(getpid(), sizeof(cpuset), &cpuset) == -1) {
                printf("Set CPU affinity failue, ERROR:%s\n", strerror(errno));
                //return -1;
            }
        }

    }

    std::cout<<RobotName.toStdString()<<" runSomeBigWork1 started"<<std::endl;
    //franka::Robot robot(RobotIP);





    while(1)//1
    {
        if(IfConnected==true)
        {
            //如果开启跟随,每个周期产生指令
            if(ExtFollowerIFStartTrack==true)
            {
                Eigen::Matrix4d FrankaPose_A;
                MathWay* mathway;

                try {
                    setDefaultBehavior(*robot);

                    size_t count = 1;




                    robot->read([&count](const franka::RobotState& robot_state) {
                        // Printing to std::cout adds a delay. This is acceptable for a read loop such as this, but
                        // should not be done in a control loop.
                        std::cout << robot_state<< std::endl;
                        return count++ < 1;
                    });


                    robot->read([&count,&FrankaPose_A](const franka::RobotState& robot_state) {
                        // Printing to std::cout adds a delay. This is acceptable for a read loop such as this, but
                        // should not be done in a control loop.
                        std::cout << robot_state<< std::endl;
                        FrankaPose_A << robot_state.O_T_EE[0],robot_state.O_T_EE[4],robot_state.O_T_EE[8],robot_state.O_T_EE[12],
                            robot_state.O_T_EE[1],robot_state.O_T_EE[5],robot_state.O_T_EE[9],robot_state.O_T_EE[13],
                            robot_state.O_T_EE[2],robot_state.O_T_EE[6],robot_state.O_T_EE[10],robot_state.O_T_EE[14],
                            robot_state.O_T_EE[3],robot_state.O_T_EE[7],robot_state.O_T_EE[11],robot_state.O_T_EE[15];
                        return count++ < 1;
                    });

                } catch (franka::Exception const& e) {
                    std::cout << e.what() << std::endl;
                    //std::cout << "Running error recovery..." << std::endl;
                    robot->automaticErrorRecovery();

                }
                std::cout<<".............."<<std::endl;
                try
                {

                    double time = 0;

                    //franka::Robot robot("192.168.65.110");


                    franka::limitRate(10.0,20.0,2000.0,10.0,10.0,10.0);
                    //     * @param[in] max_velocity Per-joint maximum allowed velocity.
                    //     * @param[in] max_acceleration Per-joint maximum allowed acceleration.
                    //     * @param[in] max_jerk Per-joint maximum allowed jerk.
                    //     * @param[in] commanded_velocities Commanded joint velocity of the current time step.
                    //     * @param[in] last_commanded_velocities Commanded joint velocities of the previous time step.
                    //     * @param[in] last_commanded_accelerations Commanded joint accelerations of the previous time step.

                    //        FrankaPose_A <<0.0494819,0.0963342,0.994118,0.44853,
                    //        -0.0124414,-0.995201,0.0970584,-0.366969,
                    //        0.998698,-0.0171709,-0.0480459,0.191117,
                    //        0,0,0,1;
                    std::cout <<  franka::kMaxTranslationalJerk << std::endl;

                    Eigen::Matrix4d FrankaToEnd;
                    FrankaToEnd <<1,0,0,-0.004918090894842,
                        0,0,1,0.420591628068343,
                        0,-1,0,0.033858874920435,
                        0,0,0,1;

                    Eigen::Matrix4d FrankaToEEF;
                    FrankaToEEF <<1,0,0,-0.004918090894842,
                        0,0,1,0,
                        0,-1,0,0.033858874920435,
                        0,0,0,1;

                    Eigen::Matrix4d FrankaToRCM;
                    FrankaToRCM <<1,0,0,-0.004918090894842,
                        0,0,1,0.420591628068343-0.2,
                        0,-1,0,0.033858874920435,
                        0,0,0,1;

                    Eigen::Matrix4d EndToCam;
                    EndToCam <<1,0,0,0,
                        0,cos(30*pi/180),-sin(30*pi/180),0,
                        0,sin(30*pi/180),cos(30*pi/180),0,
                        0,0,0,1;

                    Eigen::Matrix4d EEFToFranka=FrankaToEEF.inverse();

                    //std::cout<<FrankaToEEF<<std::endl;
                    //std::cout<<EEFToFranka<<std::endl;

                    Eigen::Vector3d Franka_Wp_A(FrankaPose_A(0,3),FrankaPose_A(1,3),FrankaPose_A(2,3));

                    Eigen::Matrix4d End_M;
                    Eigen::Matrix4d EEF_M;
                    Eigen::Matrix4d RCM_M;

                    End_M=FrankaPose_A*FrankaToEnd;
                    EEF_M=FrankaPose_A*FrankaToEEF;
                    RCM_M=FrankaPose_A*FrankaToRCM;

                    Eigen::Vector3d End_Wp_A(End_M(0,3),End_M(1,3),End_M(2,3));
                    Eigen::Vector3d EEF_Wp(EEF_M(0,3),EEF_M(1,3),EEF_M(2,3));
                    Eigen::Vector3d RCM_Wp(RCM_M(0,3),RCM_M(1,3),RCM_M(2,3));
                    RCMPoint=RCM_Wp;
                    Eigen::Matrix4d Rt;
                    Rt=FrankaPose_A*FrankaToEnd*EndToCam;


                    double L=0.420591628068343;

                    double acc_x=0,vec_x=0,det_a=0,det_x=0;

                    double kexi=2;
                    double Wn=10*2*pi;
                    double fs=1000;


                    double Den_b=10,Den_c=10,Den_d=5;
                    double k1=Den_b+Den_d;
                    double k2=Den_b*Den_d+Den_c;
                    double k3=Den_c*Den_d;
                    Eigen::Vector3d Numz(Wn*Wn/(4*fs*fs+4*kexi*Wn*fs+Wn*Wn),2*Wn*Wn/(4*fs*fs+4*kexi*Wn*fs+Wn*Wn),Wn*Wn/(4*fs*fs+4*kexi*Wn*fs+Wn*Wn));
                    Eigen::Vector2d Denz(-(5*Wn*Wn-8*fs*fs)/(4*fs*fs+4*kexi*Wn*fs+Wn*Wn),-(4*fs*fs-4*kexi*Wn*fs+Wn*Wn)/(4*fs*fs+4*kexi*Wn*fs+Wn*Wn));
                    double k=(1-Denz(0)-Denz(1))/(Numz(0)+Numz(1)+Numz(2));
                    double ylast=0,yllast=0,ulast=0,ullast=0;
                    double thr=pi/30;


                    std::cout <<"11111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 "<<std::endl;

                    Eigen::Vector3d End_pz=RCM(EEF_Wp,RCM_Wp,L);
                    Eigen::Vector4d End_p(End_pz(0),End_pz(1),End_pz(2),1);
                    Eigen::Vector4d End_pp=Rt*End_p;
                    double positionx_input=0;


                    double position_input_X=0;
                    double controller_output_X=0;

                    double position_input_Y=0;
                    double controller_output_Y=0;

                    double position_input_Z=0;
                    double controller_output_Z=0;

                    double controller_output=0;
                    int t=0;

                    time=0;
                    double det_time;
                    double _X=Franka_Wp_A(0);
                    double _Y=Franka_Wp_A(1);
                    double _Z=Franka_Wp_A(2);
                    double acc_X=0,vec_X=0,deta_X=0,det_X=0;
                    double acc_Y=0,vec_Y=0,deta_Y=0,det_Y=0;
                    double acc_Z=0,vec_Z=0,deta_Z=0,det_Z=0;
                    Eigen::Vector3d x_base;
                    x_base<<Rt(0,1),Rt(1,1),Rt(2,1);
                    std::cout << FrankaPose_A << std::endl;

                    //
                    Eigen::Matrix4d newrobot;
                    Eigen::Vector3d newEEFp(0,0,0);
                    Eigen::Matrix4d newEEF;

                    double cam_dx=0.05;
                    double cam_dy=0.05;
                    double cam_f=0.3;
                    double cam_fx=cam_f/cam_dx;
                    double cam_fy=cam_f/cam_dy;
                    double cam_z=0.5;

                    Eigen::Vector3d pointuv;

                    Eigen::Matrix3d pix2pix;
                    pix2pix<<cam_dx,0,0,0,cam_dy,0,0,0,1;

                    Eigen::Vector3d pointp;

                    Eigen::Vector4d pointc;

                    Eigen::Vector4d pointw;

                    Eigen::Vector4d pointcam;

                    Eigen::Vector3d pointcamp;

                    Eigen::Matrix4d Cam2fig;

                    Eigen::Matrix4d RobottoCam=EndToCam*FrankaToEnd*RobotPoseRealTime;
                    std::cout<<Rt<<std::endl;
                    std::cout<<RobottoCam<<std::endl;
                    pointp<<0,0,1;
                    pointc<<pointp(0)*cam_z/cam_fx,pointp(1)*cam_z/cam_fy,cam_z,1;
                    std::cout<<pointc<<std::endl;
                    pointw=RobottoCam.inverse()*pointc;
                    std::cout<<RobottoCam.inverse()<<std::endl;
                    std::cout<<pointw<<std::endl;
                    Cam2fig<<1,0,0,End_Wp_A(0)-pointw(0),
                        0,1,0,End_Wp_A(1)-pointw(1),
                        0,0,1,End_Wp_A(2)-pointw(2),
                        0,0,0,1;
                    std::cout<<Cam2fig<<std::endl;

                    pointcam=Cam2fig*pointw;

                    pointcamp<<pointcam(0),pointcam(1),pointcam(2);

                    newEEFp=mathway->RCM(pointcamp,RCM_Wp,L);

                    newEEF=EEF_M;
                    newEEF(0,3)=newEEFp(0);
                    newEEF(1,3)=newEEFp(1);
                    newEEF(2,3)=newEEFp(2);

                    newrobot=newEEF*EEFToFranka;

                    std::cout<<EEFToFranka<<std::endl;
                    std::cout<<Franka_Wp_A<<std::endl;
                    std::cout<<newEEF<<std::endl;
                    std::cout<<newrobot<<std::endl;
                    std::ofstream outfile1("../outtest1.txt", ios::trunc);
                    //                        while(1)waitKey(1000000000);
                    //
                    double lasttestvalue_X=hardware_variable::instance()->sHapticRunData[1].position.x();
                    double lasttestvalue_Y=hardware_variable::instance()->sHapticRunData[1].position.y();
                    double lasttestvalue_Z=hardware_variable::instance()->sHapticRunData[1].position.z();

                    //cartesian_velocities	Desired Cartesian velocity w.r.t. O-frame {dx in [m/s], dy in [m/s], dz in [m/s], omegax in [rad/s], omegay in [rad/s], omegaz in [rad/s]}.
                    //CartesianPose
                    robot->control([&time,&mathway,&outfile1,&x_base,&RCM_Wp,&Rt,&L,&t,&EEFToFranka,&FrankaToEnd,&EndToCam,&FrankaToEEF,
                                    &_X,&_Y,&_Z,&acc_X,&vec_X,&deta_X,&det_X, &acc_Y,&vec_Y,&deta_Y,&det_Y, &acc_Z,&vec_Z,&deta_Z,&det_Z,
                                    &k1,&k2,&k3,&position_input_X,&controller_output_X,&position_input_Y,&controller_output_Y,&position_input_Z,&controller_output_Z,
                                    &lasttestvalue_X,&lasttestvalue_Y,&lasttestvalue_Z,
                                    &det_time,&Franka_Wp_A,&FrankaPose_A,&newEEF,&newEEFp,&newrobot](const franka::RobotState&,
                                               franka::Duration period) -> franka::CartesianPose {
                        det_time=period.toSec();
                        time += det_time;

                        if (det_time>0.001){
                            std::cout<<"sssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssss"<<std::endl;


                        }

                        double _X=Franka_Wp_A(0);
                        double _Y=Franka_Wp_A(1);
                        double _Z=Franka_Wp_A(2);

                        Rt=FrankaPose_A*FrankaToEnd*EndToCam;
                        x_base<<Rt(0,1),Rt(1,1),Rt(2,1);

                        Eigen::Matrix4d Tar;


                        if(time<3)
                        {
                            position_input_X=0;
                            position_input_Y=0;
                            position_input_Z=0;
                        }
                        else
                        {
                            position_input_X=hardware_variable::instance()->sHapticRunData[1].position.x()-lasttestvalue_X;;
                            position_input_Y=hardware_variable::instance()->sHapticRunData[1].position.y()-lasttestvalue_Y;
                            position_input_Z=0.01*(hardware_variable::instance()->sHapticRunData[1].position.z()-lasttestvalue_Z);
                        }

                        //                            if(time<1)
                        //                            {
                        //                                position_input_X=0;
                        //                                position_input_Y=0;
                        //                                position_input_Z=0;
                        //                            }
                        //                            else
                        //                            {
                        //                                position_input_X=0.001*hardware_variable::instance()->Point.x;
                        //                                position_input_Y=0.001*hardware_variable::instance()->Point.y;
                        ////                                testvalueY=hardware_variable::instance()->m4dNext(1,3);
                        ////                                testvalueZ=hardware_variable::instance()->m4dNext(2,3);
                        //                                std::cout<<"AAAAAAAAAAAAAAAAAAAAAAAAAAAAAA2 testvalueXYZ="<<testvalueX<<","<<testvalueY<<","<<testvalueZ<<std::endl;
                        //                            }


                        /////////////////////
                        double err_X=position_input_X-det_X;
                        if(std::abs(err_X)>=0.05) err_X=0.05*err_X/std::abs(err_X);

                        controller_output_X=k3*(err_X)-k1*acc_X-k2*vec_X;
                        if(std::abs(controller_output_X)>1) deta_X=1*controller_output_X/std::abs(controller_output_X);
                        else deta_X=controller_output_X;
                        acc_X+=det_time*deta_X;
                        if(std::abs(acc_X)>=10) acc_X=4*acc_X/std::abs(acc_X);
                        vec_X+=det_time*acc_X;
                        if(std::abs(vec_X)>=0.8) vec_X=0.8*vec_X/std::abs(vec_X);
                        det_X+=det_time*vec_X;
                        //                            det_X=det_time*vec_X;

                        _X+=det_X;


                        /////////////////////
                        double err_Y=position_input_Y-det_Y;
                        if(std::abs(err_Y)>=0.05) err_Y=0.05*err_Y/std::abs(err_Y);

                        controller_output_Y=k3*(err_Y)-k1*acc_Y-k2*vec_Y;
                        if(std::abs(controller_output_Y)>1) deta_Y=1*controller_output_Y/std::abs(controller_output_Y);
                        else deta_Y=controller_output_Y;
                        acc_Y+=det_time*deta_Y;
                        if(std::abs(acc_Y)>=10) acc_Y=4*acc_Y/std::abs(acc_Y);
                        vec_Y+=det_time*acc_Y;
                        if(std::abs(vec_Y)>=0.8) vec_Y=0.8*vec_Y/std::abs(vec_Y);
                        det_Y+=det_time*vec_Y;
                        //                            det_Y=det_time*vec_Y;

                        _Y+=det_Y;

                        /////////////////////
                        double err_Z=position_input_Z-det_Z;
                        if(std::abs(err_Z)>=0.05) err_Z=0.05*err_Z/std::abs(err_Z);

                        controller_output_Z=k3*(err_Z)-k1*acc_Z-k2*vec_Z;
                        if(std::abs(controller_output_Z)>1) deta_Z=1*controller_output_Z/std::abs(controller_output_Z);
                        else deta_Z=controller_output_Z;
                        acc_Z+=det_time*deta_Z;
                        if(std::abs(acc_Z)>=10) acc_Z=4*acc_Z/std::abs(acc_Z);
                        vec_Z+=det_time*acc_Z;
                        if(std::abs(vec_Z)>=0.8) vec_Z=0.8*vec_Z/std::abs(vec_Z);
                        det_Z+=det_time*vec_Z;
                        //                            det_Z=det_time*vec_Z;

                        _Z+=det_Z;

                        t=t+1;

                        Eigen::Matrix4d Rot_X;
                        Rot_X<<1,0,0,0,
                            0,cos(200*det_X*pi/180),-sin(200*det_X*pi/180),0,
                            0,sin(200*det_X*pi/180),cos(200*det_X*pi/180),0,
                            0,0,0,1;

                        Eigen::Matrix4d Rot_Y;
                        Rot_Y<<cos(200*det_Y*pi/180),0,sin(200*det_Y*pi/180),0,
                            0,1,0,0,
                            -sin(200*det_Y*pi/180),0,cos(200*det_Y*pi/180),0,
                            0,0,0,1;

                        Eigen::Matrix4d Rot_Z;
                        Rot_Z<<cos(200*det_Z*pi/180),-sin(200*det_Z*pi/180),0,0,
                            sin(200*det_Z*pi/180),cos(200*det_Z*pi/180),0,0,
                            0,0,1,0,
                            0,0,0,1;

                        Eigen::Matrix4d RCM_Rotation;

                        FrankaPose_A(0,3)=_X;
                        FrankaPose_A(1,3)=_Y;
                        FrankaPose_A(2,3)=_Z;

                        newEEF=FrankaPose_A*FrankaToEEF;

                        newEEFp<<newEEF(0,3),newEEF(1,3),newEEF(2,3);

                        RCM_Rotation=mathway->GET_R_Xbase_world(x_base,newEEFp,RCMPoint);

                        outfile1<<RCMPoint<<std::endl;

                        outfile1<<x_base<<std::endl;

                        outfile1<<newEEFp<<std::endl;

                        outfile1<<Rt<<std::endl;

                        outfile1<<FrankaPose_A<<std::endl;

                        outfile1<<RCM_Rotation<<std::endl;

                        //                        Tar=FrankaPose_A*Rot_X*Rot_Y*Rot_Z;

                        //                        Tar=RCM_Rotation*EEFToFranka;
                        Tar=RCM_Rotation;
                        //                        Tar=FrankaPose_A;
                        Tar(0,3)=_X;
                        Tar(1,3)=_Y;
                        Tar(2,3)=_Z;
                        Tar(3,3)=1;

                        //                        Tar(0,3)=Franka_Wp_A(0);
                        //                        Tar(1,3)=Franka_Wp_A(1);
                        //                        Tar(2,3)=Franka_Wp_A(2);
                        //                        Tar(3,3)=1;


                        //                        Tar(0,0)=Tar(0,0)*(-1);
                        //                        Tar(1,0)=Tar(1,0)*(-1);
                        //                        Tar(2,0)=Tar(2,0)*(-1);


                        std::array<double, 16> Tar_pose={{ Tar(0,0),Tar(1,0),Tar(2,0),Tar(3,0),Tar(0,1),Tar(1,1),Tar(2,1),Tar(3,1),Tar(0,2),Tar(1,2),Tar(2,2),Tar(3,2),Tar(0,3),Tar(1,3),Tar(2,3),Tar(3,3)}};

                        std::cout<<Tar<<std::endl;

                        franka::CartesianPose output1{Tar(0,0),Tar(1,0),Tar(2,0),Tar(3,0),Tar(0,1),Tar(1,1),Tar(2,1),Tar(3,1),Tar(0,2),Tar(1,2),Tar(2,2),Tar(3,2),Tar(0,3),Tar(1,3),Tar(2,3),Tar(3,3)};

                        franka::CartesianPose output  =Tar_pose;

                        //                        std::cout <<"output0= "<<std::get<0>(output.O_T_EE)<<std::endl;// zhe ge ke yi shu chu output
                        //                        std::cout <<"output1= "<<std::get<1>(output.O_T_EE)<<std::endl;// zhe ge ke yi shu chu output
                        //                        std::cout <<"output2= "<<std::get<2>(output.O_T_EE)<<std::endl;// zhe ge ke yi shu chu output

                        //franka::CartesianVelocities output = {{ 0.0, 0.0,v_now, 0.0, 0.0, 0.0}};
                        std::cout <<_X<<", "<<_Y<<", "<<_Z<<", "<<1<<std::endl;
                        //std::cout <<Tar(0,0)<<' '<<Tar(0,1)<<' '<<Tar(0,2)<<' '<<Tar(0,3)<<' '<<Tar(1,0)<<' '<<Tar(1,1)<<' '<<Tar(1,2)<<' '<<Tar(1,3)<<' '<<Tar(2,0)<<' '<<Tar(2,1)<<' '<<Tar(2,2)<<' '<<Tar(2,3)<<' '<<Tar(3,0)<<' '<<Tar(3,1)<<' '<<Tar(3,2)<<' '<<Tar(3,3) << std::endl;
                        //std::cout <<det_a<<", "<<acc_x<<", "<<vec_x<<", "<<det_x<<std::endl;
                        //std::cout <<positionx_input<<", "<<det_x<<", "<<det_a<<std::endl;
                        //std::cout <<"t:"<<t <<std::endl;
                        //std::cout <<"time:"<<time <<std::endl;


                        if (time>= 1000) {
                            std::cout << std::endl << "Finished motion, shutting down example" << std::endl;
                            return franka::MotionFinished(output);
                        }

                        //std::cout<<"output="<<output(0,0)<<std::endl;
                        //                    std::cout <<"output=="<<(double*)&output<<std::endl;

                        return output1;
                    });




                    std::cout << "Done1." << std::endl;
                } catch (franka::Exception const& e) {
                    std::cout << e.what() << std::endl;
                    //std::cout << "Running error recovery.............................................................................." << std::endl;
                    robot->automaticErrorRecovery();

                }
            }
            //不开启跟随的情况下，也输出采集的数据
            else
            {
                if(command==0)
                {
                    try
                    {
                        size_t count = 1;
                        double time = 0;

                        //franka::Robot robot("192.168.65.110");
                        setDefaultBehavior(*robot);
                        Eigen::Matrix4d FrankaPose_A;
                        robot->read([&count,&FrankaPose_A](const franka::RobotState& robot_state) {
                            // Printing to std::cout adds a delay. This is acceptable for a read loop such as this, but
                            // should not be done in a control loop.
                            //std::cout << robot_state<< std::endl;
                            //std::cout << std::get<0>(robot_state.O_T_EE)<< std::endl;
                            FrankaPose_A << robot_state.O_T_EE[0],robot_state.O_T_EE[4],robot_state.O_T_EE[8],robot_state.O_T_EE[12],
                                robot_state.O_T_EE[1],robot_state.O_T_EE[5],robot_state.O_T_EE[9],robot_state.O_T_EE[13],
                                robot_state.O_T_EE[2],robot_state.O_T_EE[6],robot_state.O_T_EE[10],robot_state.O_T_EE[14],
                                robot_state.O_T_EE[3],robot_state.O_T_EE[7],robot_state.O_T_EE[11],robot_state.O_T_EE[15];


                            tick++;
                            if(tick>100)
                            {
                                tick=0;
                                std::cout<<"FrankaRobotControlV2::TCPPose="<<FrankaPose_A<<std::endl;
                                std::cout <<"FrankaRobotControlV2::JointPos="<< robot_state.q_d[0]<<","<< robot_state.q_d[1]<<","<< robot_state.q_d[2]<<","<< robot_state.q_d[3]<<","<< robot_state.q_d[4]<<","<< robot_state.q_d[5]<<","<< robot_state.q_d[6]<<","<< std::endl;
                                hardware_variable::instance()->robot_m4d=FrankaPose_A;
                                hardware_variable::instance()->robot_q_d=robot_state.q_d;
                            }

                            return count++ < 1;
                        });
                    }
                    catch (franka::Exception const& e)
                    {
                        std::cout << e.what() << std::endl;
                        std::cout << "Running error recovery..." << std::endl;
                        robot->automaticErrorRecovery();
                    }
                }

                if(command==1)
                {
                    try {
                        //setDefaultBehavior(robot);
                        std::array<double, 7> q_goal;
                        //        for (size_t i = 0; i < 7; i++) {
                        //            //q_goal[i] = std::stod(argv[i + 2]);
                        //            q_goal[i] = pos.DATA[i];
                        //        }
                        q_goal[0]=command_J0;//j0;
                        q_goal[1]=command_J1;//j1;
                        q_goal[2]=command_J2;//j2;
                        q_goal[3]=command_J3;//j3;
                        q_goal[4]=command_J4;//j4;
                        q_goal[5]=command_J5;//j5;
                        q_goal[6]=command_J6;//j6;

                        double speed_factor = 0.5;//std::stod(argv[9])

                        // Set additional parameters always before the control loop, NEVER in the control loop!
                        // Set collision behavior.
                        //        robot->setCollisionBehavior(
                        //            {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}},
                        //            {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}},
                        //            {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}},
                        //            {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}});

                        MotionGenerator motion_generator(speed_factor, q_goal);
                        //        std::cout << "WARNING: This example will move the robot! "
                        //                  << "Please make sure to have the user stop button at hand!" << std::endl
                        //                  << "Press Enter to continue..." << std::endl;
                        //std::cin.ignore();
                        robot->control(motion_generator);
                        std::cout << "Motion finished" << std::endl;
                        command=0;

                    } catch (const franka::Exception& e) {
                        std::cout << e.what() << std::endl;
                    }

                }
            }

        }

        //如果开启跟随进入死循环，则这里不应该被执行到
        struct timespec wakeupTime;
        const struct timespec cycletime = {0, PERIOD_NS_franka};
        clock_gettime(CLOCK_TO_USE, &wakeupTime);
        wakeupTime = timespec_add_franka(wakeupTime, cycletime);
        clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL);

    }

    std::cout<<RobotName.toStdString()<<" runSomeBigWork1 stoped"<<std::endl;
}

void FrankaRobotControlV2::runSomeBigWork2(){
    //with RCM in segma
    if(true)
    {
        pid_t pid = getpid();
        if (setpriority(PRIO_PROCESS, pid, -19))
            fprintf(stderr, "Warning: Failed to set priority: %s\n",strerror(errno));

        /* Create cyclic RT-thread */
        struct sched_param param;
        param.sched_priority = sched_get_priority_max(SCHED_FIFO) - 1;
        if (sched_setscheduler(0, SCHED_FIFO, &param) == -1) {
            printf("ERROR IN SETTING THE SCHEDULER");
            perror("errno");
            //return -1;
        }

        if(false)//fun_globalvariable::instance()->IfWorkModeReal==true
        {
            cpu_set_t cpuset;
            CPU_ZERO(&cpuset);
            CPU_SET(3, &cpuset);
            if (sched_setaffinity(getpid(), sizeof(cpuset), &cpuset) == -1) {
                printf("Set CPU affinity failue, ERROR:%s\n", strerror(errno));
                //return -1;
            }
        }

    }

    std::cout<<RobotName.toStdString()<<" runSomeBigWork1 started"<<std::endl;
    //franka::Robot robot(RobotIP);





    while(1)//1
    {
        if(IfConnected==true)
        {
            //如果开启跟随,每个周期产生指令
            if(ExtFollowerIFStartTrack==true)
            {
                Eigen::Matrix4d FrankaPose_A;
                MathWay* mathway;

                try {
                    setDefaultBehavior(*robot);

                    size_t count = 1;




                    robot->read([&count](const franka::RobotState& robot_state) {
                        // Printing to std::cout adds a delay. This is acceptable for a read loop such as this, but
                        // should not be done in a control loop.
                        std::cout << robot_state<< std::endl;
                        return count++ < 1;
                    });


                    robot->read([&count,&FrankaPose_A](const franka::RobotState& robot_state) {
                        // Printing to std::cout adds a delay. This is acceptable for a read loop such as this, but
                        // should not be done in a control loop.
                        std::cout << robot_state<< std::endl;
                        FrankaPose_A << robot_state.O_T_EE[0],robot_state.O_T_EE[4],robot_state.O_T_EE[8],robot_state.O_T_EE[12],
                            robot_state.O_T_EE[1],robot_state.O_T_EE[5],robot_state.O_T_EE[9],robot_state.O_T_EE[13],
                            robot_state.O_T_EE[2],robot_state.O_T_EE[6],robot_state.O_T_EE[10],robot_state.O_T_EE[14],
                            robot_state.O_T_EE[3],robot_state.O_T_EE[7],robot_state.O_T_EE[11],robot_state.O_T_EE[15];
                        return count++ < 1;
                    });

                } catch (franka::Exception const& e) {
                    std::cout << e.what() << std::endl;
                    //std::cout << "Running error recovery..." << std::endl;
                    robot->automaticErrorRecovery();

                }
                std::cout<<".............."<<std::endl;
                try
                {

                    double time = 0;

                    //franka::Robot robot("192.168.65.110");




                    std::cout <<  franka::kDeltaT<< std::endl;
                    std::cout <<  franka::kFactorCartesianRotationPoseInterface<< std::endl;
                    std::cout <<  franka::kMaxTranslationalJerk << std::endl;
                    std::cout <<  franka::kMaxTranslationalVelocity<< std::endl;
                    std::cout <<  franka::kMaxTranslationalAcceleration<< std::endl;
                    std::cout <<  franka::kMaxRotationalJerk<< std::endl;
                    std::cout <<  franka::kMaxRotationalVelocity<< std::endl;
                    std::cout <<  franka::kMaxRotationalAcceleration<< std::endl;



                    franka::limitRate(10.0,20.0,2000.0,10.0,10.0,10.0);
                    //     * @param[in] max_velocity Per-joint maximum allowed velocity.
                    //     * @param[in] max_acceleration Per-joint maximum allowed acceleration.
                    //     * @param[in] max_jerk Per-joint maximum allowed jerk.
                    //     * @param[in] commanded_velocities Commanded joint velocity of the current time step.
                    //     * @param[in] last_commanded_velocities Commanded joint velocities of the previous time step.
                    //     * @param[in] last_commanded_accelerations Commanded joint accelerations of the previous time step.

                    //        FrankaPose_A <<0.0494819,0.0963342,0.994118,0.44853,
                    //        -0.0124414,-0.995201,0.0970584,-0.366969,
                    //        0.998698,-0.0171709,-0.0480459,0.191117,
                    //        0,0,0,1;
                    std::cout <<  franka::kMaxTranslationalJerk << std::endl;

                    Eigen::Matrix4d FrankaToEnd;
                    FrankaToEnd <<1,0,0,-0.004918090894842,
                        0,0,1,0.420591628068343,
                        0,-1,0,0.033858874920435,
                        0,0,0,1;

                    Eigen::Matrix4d FrankaToEEF;
                    FrankaToEEF <<1,0,0,-0.004918090894842,
                        0,0,1,0,
                        0,-1,0,0.033858874920435,
                        0,0,0,1;

                    Eigen::Matrix4d FrankaToRCM;
                    FrankaToRCM <<1,0,0,-0.004918090894842,
                        0,0,1,0.420591628068343-0.2,
                        0,-1,0,0.033858874920435,
                        0,0,0,1;

                    Eigen::Matrix4d EndToCam;
                    EndToCam <<1,0,0,0,
                        0,cos(30*pi/180),-sin(30*pi/180),0,
                        0,sin(30*pi/180),cos(30*pi/180),0,
                        0,0,0,1;

                    Eigen::Matrix4d EEFToFranka=FrankaToEEF.inverse();

                    //std::cout<<FrankaToEEF<<std::endl;
                    //std::cout<<EEFToFranka<<std::endl;

                    Eigen::Vector3d Franka_Wp_A(FrankaPose_A(0,3),FrankaPose_A(1,3),FrankaPose_A(2,3));

                    Eigen::Matrix4d End_M;
                    Eigen::Matrix4d EEF_M;
                    Eigen::Matrix4d RCM_M;

                    End_M=FrankaPose_A*FrankaToEnd;
                    EEF_M=FrankaPose_A*FrankaToEEF;
                    RCM_M=FrankaPose_A*FrankaToRCM;

                    Eigen::Vector3d End_Wp_A(End_M(0,3),End_M(1,3),End_M(2,3));
                    Eigen::Vector3d EEF_Wp(EEF_M(0,3),EEF_M(1,3),EEF_M(2,3));
                    Eigen::Vector3d RCM_Wp(RCM_M(0,3),RCM_M(1,3),RCM_M(2,3));
                    RCMPoint=RCM_Wp;
                    Eigen::Matrix4d Rt;
                    Rt=FrankaPose_A*FrankaToEnd*EndToCam;


                    double L=0.420591628068343;

                    double acc_x=0,vec_x=0,det_a=0,det_x=0;

                    double kexi=2;
                    double Wn=10*2*pi;
                    double fs=1000;


                    double Den_b=10,Den_c=10,Den_d=5;
                    double k1=Den_b+Den_d;
                    double k2=Den_b*Den_d+Den_c;
                    double k3=Den_c*Den_d;
                    Eigen::Vector3d Numz(Wn*Wn/(4*fs*fs+4*kexi*Wn*fs+Wn*Wn),2*Wn*Wn/(4*fs*fs+4*kexi*Wn*fs+Wn*Wn),Wn*Wn/(4*fs*fs+4*kexi*Wn*fs+Wn*Wn));
                    Eigen::Vector2d Denz(-(5*Wn*Wn-8*fs*fs)/(4*fs*fs+4*kexi*Wn*fs+Wn*Wn),-(4*fs*fs-4*kexi*Wn*fs+Wn*Wn)/(4*fs*fs+4*kexi*Wn*fs+Wn*Wn));
                    double k=(1-Denz(0)-Denz(1))/(Numz(0)+Numz(1)+Numz(2));
                    double ylast=0,yllast=0,ulast=0,ullast=0;
                    double thr=pi/30;


                    std::cout <<"11111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 "<<std::endl;

                    Eigen::Vector3d End_pz=RCM(EEF_Wp,RCM_Wp,L);
                    Eigen::Vector4d End_p(End_pz(0),End_pz(1),End_pz(2),1);
                    Eigen::Vector4d End_pp=Rt*End_p;
                    double positionx_input=0;


                    double position_input_X=0;
                    double controller_output_X=0;

                    double position_input_Y=0;
                    double controller_output_Y=0;

                    double position_input_Z=0;
                    double controller_output_Z=0;

                    double controller_output=0;
                    int t=0;

                    time=0;
                    double det_time;
                    double _X=Franka_Wp_A(0);
                    double _Y=Franka_Wp_A(1);
                    double _Z=Franka_Wp_A(2);
                    double acc_X=0,vec_X=0,deta_X=0,det_X=0;
                    double acc_Y=0,vec_Y=0,deta_Y=0,det_Y=0;
                    double acc_Z=0,vec_Z=0,deta_Z=0,det_Z=0;
                    Eigen::Vector3d x_base;
                    x_base<<Rt(0,1),Rt(1,1),Rt(2,1);
                    std::cout << FrankaPose_A << std::endl;

                    //
                    Eigen::Matrix4d newrobot;
                    Eigen::Vector3d newEEFp(0,0,0);
                    Eigen::Matrix4d newEEF;

                    double cam_dx=0.05;
                    double cam_dy=0.05;
                    double cam_f=0.3;
                    double cam_fx=cam_f/cam_dx;
                    double cam_fy=cam_f/cam_dy;
                    double cam_z=0.5;

                    Eigen::Vector3d pointuv;

                    Eigen::Matrix3d pix2pix;
                    pix2pix<<cam_dx,0,0,0,cam_dy,0,0,0,1;

                    Eigen::Vector3d pointp;

                    Eigen::Vector4d pointc;

                    Eigen::Vector4d pointw;

                    Eigen::Vector4d pointcam;

                    Eigen::Vector3d pointcamp;

                    Eigen::Matrix4d Cam2fig;

                    Eigen::Matrix4d RobottoCam=EndToCam*FrankaToEnd*RobotPoseRealTime;
                    std::cout<<Rt<<std::endl;
                    std::cout<<RobottoCam<<std::endl;
                    pointp<<0,0,1;
                    pointc<<pointp(0)*cam_z/cam_fx,pointp(1)*cam_z/cam_fy,cam_z,1;
                    std::cout<<pointc<<std::endl;
                    pointw=RobottoCam.inverse()*pointc;
                    std::cout<<RobottoCam.inverse()<<std::endl;
                    std::cout<<pointw<<std::endl;
                    Cam2fig<<1,0,0,End_Wp_A(0)-pointw(0),
                        0,1,0,End_Wp_A(1)-pointw(1),
                        0,0,1,End_Wp_A(2)-pointw(2),
                        0,0,0,1;
                    std::cout<<Cam2fig<<std::endl;

                    pointcam=Cam2fig*pointw;

                    pointcamp<<pointcam(0),pointcam(1),pointcam(2);

                    newEEFp=mathway->RCM(pointcamp,RCM_Wp,L);

                    newEEF=EEF_M;
                    newEEF(0,3)=newEEFp(0);
                    newEEF(1,3)=newEEFp(1);
                    newEEF(2,3)=newEEFp(2);

                    newrobot=newEEF*EEFToFranka;

                    std::cout<<EEFToFranka<<std::endl;
                    std::cout<<Franka_Wp_A<<std::endl;
                    std::cout<<newEEF<<std::endl;
                    std::cout<<newrobot<<std::endl;
                    std::ofstream outfile1("../outtest1.txt", ios::trunc);
                    //                        while(1)waitKey(1000000000);
                    //


                    //cartesian_velocities	Desired Cartesian velocity w.r.t. O-frame {dx in [m/s], dy in [m/s], dz in [m/s], omegax in [rad/s], omegay in [rad/s], omegaz in [rad/s]}.
                    //CartesianPose
                    robot->control([&time,&mathway,&outfile1,&x_base,&RCM_Wp,&Rt,&L,&t,&EEFToFranka,&FrankaToEnd,&EndToCam,&FrankaToEEF,
                                    &_X,&_Y,&_Z,&acc_X,&vec_X,&deta_X,&det_X, &acc_Y,&vec_Y,&deta_Y,&det_Y, &acc_Z,&vec_Z,&deta_Z,&det_Z,
                                    &k1,&k2,&k3,&position_input_X,&controller_output_X,&position_input_Y,&controller_output_Y,&position_input_Z,&controller_output_Z,
                                    &det_time,&Franka_Wp_A,&FrankaPose_A,&newEEF,&newEEFp,&newrobot](const franka::RobotState& robot_state,
                                               franka::Duration period) -> franka::CartesianPose {
                        det_time=period.toSec();
                        time += det_time;

                        if (det_time>0.001){
                            std::cout<<"sssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssss"<<std::endl;
                            //                            FrankaPose_A << robot_state.O_T_EE[0],robot_state.O_T_EE[4],robot_state.O_T_EE[8],robot_state.O_T_EE[12],
                            //                                    robot_state.O_T_EE[1],robot_state.O_T_EE[5],robot_state.O_T_EE[9],robot_state.O_T_EE[13],
                            //                                    robot_state.O_T_EE[2],robot_state.O_T_EE[6],robot_state.O_T_EE[10],robot_state.O_T_EE[14],
                            //                                    robot_state.O_T_EE[3],robot_state.O_T_EE[7],robot_state.O_T_EE[11],robot_state.O_T_EE[15];

                        }

                        double _X=Franka_Wp_A(0);
                        double _Y=Franka_Wp_A(1);
                        double _Z=Franka_Wp_A(2);

                        Rt=FrankaPose_A*FrankaToEnd*EndToCam;
                        x_base<<Rt(0,1),Rt(1,1),Rt(2,1);

                        Eigen::Matrix4d Tar;


                        if(time<3)
                        {
                            position_input_X=0;
                            position_input_Y=0;
                            position_input_Z=0;
                        }
                        else
                        {
                            position_input_X=testvalueX;
                            position_input_Y=testvalueY;
                            position_input_Z=testvalueZ;
                        }

                        //                            if(time<1)
                        //                            {
                        //                                position_input_X=0;
                        //                                position_input_Y=0;
                        //                                position_input_Z=0;
                        //                            }
                        //                            else
                        //                            {
                        //                                position_input_X=0.001*hardware_variable::instance()->Point.x;
                        //                                position_input_Y=0.001*hardware_variable::instance()->Point.y;
                        ////                                testvalueY=hardware_variable::instance()->m4dNext(1,3);
                        ////                                testvalueZ=hardware_variable::instance()->m4dNext(2,3);
                        //                                std::cout<<"AAAAAAAAAAAAAAAAAAAAAAAAAAAAAA2 testvalueXYZ="<<testvalueX<<","<<testvalueY<<","<<testvalueZ<<std::endl;
                        //                            }


                        /////////////////////
                        double err_X=position_input_X-det_X;
                        if(std::abs(err_X)>=0.05) err_X=0.05*err_X/std::abs(err_X);

                        controller_output_X=k3*(err_X)-k1*acc_X-k2*vec_X;
                        if(std::abs(controller_output_X)>1) deta_X=1*controller_output_X/std::abs(controller_output_X);
                        else deta_X=controller_output_X;
                        acc_X+=det_time*deta_X;
                        if(std::abs(acc_X)>=10) acc_X=4*acc_X/std::abs(acc_X);
                        vec_X+=det_time*acc_X;
                        if(std::abs(vec_X)>=0.8) vec_X=0.8*vec_X/std::abs(vec_X);
                        det_X+=det_time*vec_X;
                        //                            det_X=det_time*vec_X;

                        _X+=det_X;


                        /////////////////////
                        double err_Y=position_input_Y-det_Y;
                        if(std::abs(err_Y)>=0.05) err_Y=0.05*err_Y/std::abs(err_Y);

                        controller_output_Y=k3*(err_Y)-k1*acc_Y-k2*vec_Y;
                        if(std::abs(controller_output_Y)>1) deta_Y=1*controller_output_Y/std::abs(controller_output_Y);
                        else deta_Y=controller_output_Y;
                        acc_Y+=det_time*deta_Y;
                        if(std::abs(acc_Y)>=10) acc_Y=4*acc_Y/std::abs(acc_Y);
                        vec_Y+=det_time*acc_Y;
                        if(std::abs(vec_Y)>=0.8) vec_Y=0.8*vec_Y/std::abs(vec_Y);
                        det_Y+=det_time*vec_Y;
                        //                            det_Y=det_time*vec_Y;

                        _Y+=det_Y;

                        /////////////////////
                        double err_Z=position_input_Z-det_Z;
                        if(std::abs(err_Z)>=0.05) err_Z=0.05*err_Z/std::abs(err_Z);

                        controller_output_Z=k3*(err_Z)-k1*acc_Z-k2*vec_Z;
                        if(std::abs(controller_output_Z)>1) deta_Z=1*controller_output_Z/std::abs(controller_output_Z);
                        else deta_Z=controller_output_Z;
                        acc_Z+=det_time*deta_Z;
                        if(std::abs(acc_Z)>=10) acc_Z=4*acc_Z/std::abs(acc_Z);
                        vec_Z+=det_time*acc_Z;
                        if(std::abs(vec_Z)>=0.8) vec_Z=0.8*vec_Z/std::abs(vec_Z);
                        det_Z+=det_time*vec_Z;
                        //                            det_Z=det_time*vec_Z;

                        _Z+=det_Z;

                        t=t+1;

                        Eigen::Matrix4d Rot_X;
                        Rot_X<<1,0,0,0,
                            0,cos(200*det_X*pi/180),-sin(200*det_X*pi/180),0,
                            0,sin(200*det_X*pi/180),cos(200*det_X*pi/180),0,
                            0,0,0,1;

                        Eigen::Matrix4d Rot_Y;
                        Rot_Y<<cos(200*det_Y*pi/180),0,sin(200*det_Y*pi/180),0,
                            0,1,0,0,
                            -sin(200*det_Y*pi/180),0,cos(200*det_Y*pi/180),0,
                            0,0,0,1;

                        Eigen::Matrix4d Rot_Z;
                        Rot_Z<<cos(200*det_Z*pi/180),-sin(200*det_Z*pi/180),0,0,
                            sin(200*det_Z*pi/180),cos(200*det_Z*pi/180),0,0,
                            0,0,1,0,
                            0,0,0,1;

                        Eigen::Matrix4d RCM_Rotation;

                        FrankaPose_A(0,3)=_X;
                        FrankaPose_A(1,3)=_Y;
                        FrankaPose_A(2,3)=_Z;

                        newEEF=FrankaPose_A*FrankaToEEF;

                        newEEFp<<newEEF(0,3),newEEF(1,3),newEEF(2,3);

                        RCM_Rotation=mathway->GET_R_Xbase_world(x_base,newEEFp,RCMPoint);

                        outfile1<<RCMPoint<<std::endl;

                        outfile1<<x_base<<std::endl;

                        outfile1<<newEEFp<<std::endl;

                        outfile1<<Rt<<std::endl;

                        outfile1<<FrankaPose_A<<std::endl;

                        outfile1<<RCM_Rotation<<std::endl;

                        //                        Tar=FrankaPose_A*Rot_X*Rot_Y*Rot_Z;

                        //                        Tar=RCM_Rotation*EEFToFranka;
                        Tar=RCM_Rotation;
                        //                        Tar=FrankaPose_A;
                        Tar(0,3)=_X;
                        Tar(1,3)=_Y;
                        Tar(2,3)=_Z;
                        Tar(3,3)=1;

                        //                        Tar(0,3)=Franka_Wp_A(0);
                        //                        Tar(1,3)=Franka_Wp_A(1);
                        //                        Tar(2,3)=Franka_Wp_A(2);
                        //                        Tar(3,3)=1;


                        //                        Tar(0,0)=Tar(0,0)*(-1);
                        //                        Tar(1,0)=Tar(1,0)*(-1);
                        //                        Tar(2,0)=Tar(2,0)*(-1);


                        std::array<double, 16> Tar_pose={{ Tar(0,0),Tar(1,0),Tar(2,0),Tar(3,0),Tar(0,1),Tar(1,1),Tar(2,1),Tar(3,1),Tar(0,2),Tar(1,2),Tar(2,2),Tar(3,2),Tar(0,3),Tar(1,3),Tar(2,3),Tar(3,3)}};

                        std::cout<<Tar<<std::endl;

                        franka::CartesianPose output1{Tar(0,0),Tar(1,0),Tar(2,0),Tar(3,0),Tar(0,1),Tar(1,1),Tar(2,1),Tar(3,1),Tar(0,2),Tar(1,2),Tar(2,2),Tar(3,2),Tar(0,3),Tar(1,3),Tar(2,3),Tar(3,3)};

                        franka::CartesianPose output  =Tar_pose;

                        //                        std::cout <<"output0= "<<std::get<0>(output.O_T_EE)<<std::endl;// zhe ge ke yi shu chu output
                        //                        std::cout <<"output1= "<<std::get<1>(output.O_T_EE)<<std::endl;// zhe ge ke yi shu chu output
                        //                        std::cout <<"output2= "<<std::get<2>(output.O_T_EE)<<std::endl;// zhe ge ke yi shu chu output

                        //franka::CartesianVelocities output = {{ 0.0, 0.0,v_now, 0.0, 0.0, 0.0}};
                        std::cout <<_X<<", "<<_Y<<", "<<_Z<<", "<<1<<std::endl;
                        //std::cout <<Tar(0,0)<<' '<<Tar(0,1)<<' '<<Tar(0,2)<<' '<<Tar(0,3)<<' '<<Tar(1,0)<<' '<<Tar(1,1)<<' '<<Tar(1,2)<<' '<<Tar(1,3)<<' '<<Tar(2,0)<<' '<<Tar(2,1)<<' '<<Tar(2,2)<<' '<<Tar(2,3)<<' '<<Tar(3,0)<<' '<<Tar(3,1)<<' '<<Tar(3,2)<<' '<<Tar(3,3) << std::endl;
                        //std::cout <<det_a<<", "<<acc_x<<", "<<vec_x<<", "<<det_x<<std::endl;
                        //std::cout <<positionx_input<<", "<<det_x<<", "<<det_a<<std::endl;
                        //std::cout <<"t:"<<t <<std::endl;
                        //std::cout <<"time:"<<time <<std::endl;


                        if (time>= 1000) {
                            std::cout << std::endl << "Finished motion, shutting down example" << std::endl;
                            return franka::MotionFinished(output);
                        }

                        //std::cout<<"output="<<output(0,0)<<std::endl;
                        //                    std::cout <<"output=="<<(double*)&output<<std::endl;

                        return output1;
                    });




                    std::cout << "Done1." << std::endl;
                } catch (franka::Exception const& e) {
                    std::cout << e.what() << std::endl;
                    std::cout << "Running error recovery.............................................................................." << std::endl;
                    //                    robot->automaticErrorRecovery();

                }
            }
            //不开启跟随的情况下，也输出采集的数据
            else
            {
                if(command==0)
                {
                    try
                    {
                        size_t count = 1;
                        double time = 0;

                        //franka::Robot robot("192.168.65.110");
                        setDefaultBehavior(*robot);
                        Eigen::Matrix4d FrankaPose_A;
                        robot->read([&count,&FrankaPose_A](const franka::RobotState& robot_state) {
                            // Printing to std::cout adds a delay. This is acceptable for a read loop such as this, but
                            // should not be done in a control loop.
                            //std::cout << robot_state<< std::endl;
                            //std::cout << std::get<0>(robot_state.O_T_EE)<< std::endl;
                            FrankaPose_A << robot_state.O_T_EE[0],robot_state.O_T_EE[4],robot_state.O_T_EE[8],robot_state.O_T_EE[12],
                                robot_state.O_T_EE[1],robot_state.O_T_EE[5],robot_state.O_T_EE[9],robot_state.O_T_EE[13],
                                robot_state.O_T_EE[2],robot_state.O_T_EE[6],robot_state.O_T_EE[10],robot_state.O_T_EE[14],
                                robot_state.O_T_EE[3],robot_state.O_T_EE[7],robot_state.O_T_EE[11],robot_state.O_T_EE[15];


                            tick++;
                            if(tick>100)
                            {
                                tick=0;
                                std::cout<<"FrankaRobotControlV2::TCPPose="<<FrankaPose_A<<std::endl;
                                std::cout <<"FrankaRobotControlV2::JointPos="<< robot_state.q_d[0]<<","<< robot_state.q_d[1]<<","<< robot_state.q_d[2]<<","<< robot_state.q_d[3]<<","<< robot_state.q_d[4]<<","<< robot_state.q_d[5]<<","<< robot_state.q_d[6]<<","<< std::endl;
                                hardware_variable::instance()->robot_m4d=FrankaPose_A;
                                hardware_variable::instance()->robot_q_d=robot_state.q_d;
                            }

                            return count++ < 1;
                        });
                    }
                    catch (franka::Exception const& e)
                    {
                        std::cout << e.what() << std::endl;
                        std::cout << "Running error recovery..." << std::endl;
                        robot->automaticErrorRecovery();
                    }
                }

                if(command==1)
                {
                    try {
                        //setDefaultBehavior(robot);
                        std::array<double, 7> q_goal;
                        //        for (size_t i = 0; i < 7; i++) {
                        //            //q_goal[i] = std::stod(argv[i + 2]);
                        //            q_goal[i] = pos.DATA[i];
                        //        }
                        q_goal[0]=command_J0;//j0;
                        q_goal[1]=command_J1;//j1;
                        q_goal[2]=command_J2;//j2;
                        q_goal[3]=command_J3;//j3;
                        q_goal[4]=command_J4;//j4;
                        q_goal[5]=command_J5;//j5;
                        q_goal[6]=command_J6;//j6;

                        double speed_factor = 0.5;//std::stod(argv[9])

                        // Set additional parameters always before the control loop, NEVER in the control loop!
                        // Set collision behavior.
                        //        robot->setCollisionBehavior(
                        //            {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}},
                        //            {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}},
                        //            {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}},
                        //            {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}});

                        MotionGenerator motion_generator(speed_factor, q_goal);
                        //        std::cout << "WARNING: This example will move the robot! "
                        //                  << "Please make sure to have the user stop button at hand!" << std::endl
                        //                  << "Press Enter to continue..." << std::endl;
                        //std::cin.ignore();
                        robot->control(motion_generator);
                        std::cout << "Motion finished" << std::endl;
                        command=0;

                    } catch (const franka::Exception& e) {
                        std::cout << e.what() << std::endl;
                    }

                }
            }

        }

        //如果开启跟随进入死循环，则这里不应该被执行到
        struct timespec wakeupTime;
        const struct timespec cycletime = {0, PERIOD_NS_franka};
        clock_gettime(CLOCK_TO_USE, &wakeupTime);
        wakeupTime = timespec_add_franka(wakeupTime, cycletime);
        clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL);

    }

    std::cout<<RobotName.toStdString()<<" runSomeBigWork2 stoped"<<std::endl;


}

Eigen::Matrix<double,6,7>  Jmatrix(std::array<double, 7> qsq,Eigen::Matrix4d Tlast,Eigen::Matrix3d TlastRinv){
    MathWay* mathway;
    Eigen::Matrix<double,6,7> JMatrix=Eigen::MatrixXd::Zero(6,7);
    Eigen::Matrix<double,7,1> qsqVector;
    qsqVector<<qsq[0],qsq[1],qsq[2],qsq[3],qsq[4],qsq[5],qsq[6];
    for(int j=0;j<7;j++){
        Eigen::Matrix<double,7,1>   deta;
        deta<<0,0,0,0,0,0,0;
        deta(j)=0.001;
        Eigen::Matrix<double,6,1> VECvwTthis;
        Eigen::Matrix4d TTthis;
        TTthis=mathway->frankafkine(qsqVector+deta);
        VECvwTthis=mathway->T2vw(TTthis,Tlast,TlastRinv);
        JMatrix.block(0,j,6,1)=VECvwTthis/0.001;
    }
    return JMatrix;
}


Eigen::Matrix<double,6,7> JJJmatrix(Eigen::VectorXd qsq,Eigen::Matrix4d Tlast,MathWay* mathway){

    Eigen::Matrix<double,6,7> JJmatrix=Eigen::MatrixXd::Zero(6,7);
    for(int j=0;j<7;j++){
        Eigen::Matrix<double,7,1> deta;
        deta<<0,0,0,0,0,0,0;
        deta(j)=0.0000001;
        Eigen::Matrix<double,6,1> VECvwTthis;
        Eigen::Matrix4d Tthis=mathway->frankafkine(qsq+deta);
        VECvwTthis=mathway->T2vw(Tthis,Tlast);
        JJmatrix.col(j)=VECvwTthis/0.0000001;
    }

    return JJmatrix;
}

#endif
